#include <stdlib.h>
#include <stdio.h>
#include "GL/glut.h"
#include "Bone.h"
#include "BoneList.h"

float fabs(float n)
{
	if(n < 0)
		return -n;
	return n;
}
Bone::Bone( float angle, float length )
{
	printf("Making bone\n");
	outgoingBones = NULL;
	
	currentOrientation = BoneOrientation(length, angle, 0.0, 0.0, 0.0);
	accelerating = true;
	desiredOrientation = NULL;
	angleModifier = 0.0;
	lengthModifier = 0.0;
	timeModifier = 0.0;
	
	this->textureManager = NULL;
	this->textureVertices = NULL;
	this->size = Vector2D(0,0);
	this->skinLocation = 0.0;
}
Bone::Bone( BoneOrientation currentOrientation )
{
	outgoingBones = NULL;
	this->currentOrientation = currentOrientation;
	accelerating = true;
	desiredOrientation = NULL;
	angleModifier = 0.0;
	lengthModifier = 0.0;
	timeModifier = 0.0;
	
	this->textureManager = NULL;
	this->textureVertices = NULL;
	this->size = Vector2D(0,0);
	this->skinLocation = 0.0;
}
Bone::Bone( BoneOrientation currentOrientation, TextureManager * textureManager, char * textureName, Vector2D * textureVertices, Vector2D size, float skinLocation )
{
	outgoingBones = NULL;
	if(textureManager)
	{
		this->textureManager = textureManager;
		if(!this->textureManager->LoadTexture(textureName))
		{
			textureIndex = -1;
			printf("Failed to load texture %s\n", textureName);
		}
		else
		{
			textureIndex = this->textureManager->TextureIndex(textureName);
		}
	}
	this->textureVertices = textureVertices;
	this->size = size;
	this->skinLocation = skinLocation;
}
Bone::~Bone()
{
	if(outgoingBones)
	{
		delete(outgoingBones);
		outgoingBones = NULL;
	}
	if(desiredOrientation)
	{
		delete(desiredOrientation);
		desiredOrientation = NULL;
	}
	if(textureVertices)
	{
		delete(textureVertices);
		textureVertices = NULL;
	}
}
	
void Bone::AddBone(Bone * bone)
{
	if(!outgoingBones)
		outgoingBones = new BoneList(bone);
	else
		outgoingBones->Add(bone);
}
void Bone::AddBone(float angle, float length)
{
	Bone * bone = new Bone(length, angle);
	outgoingBones->Add(bone);	
}
	
void Bone::Think(double elapsedTime)
{
	currentOrientation.SetTime(currentOrientation.GetTime() + elapsedTime);
	if(desiredOrientation && !CheckOrientation())
	{
		if(1)//currentOrientation.GetTime() < deltaOrientation.GetTime())
		{
			float angleChange = currentOrientation.GetSpeed() * elapsedTime;
			//printf("angle change = %f\n", angleChange);
			float lengthChange = currentOrientation.GetLengthSpeed() * elapsedTime;
			//printf("current angle = %f\n", currentOrientation.GetAngle());
			//printf("desired angle = %f\n", desiredOrientation->GetAngle());
			//printf("delta angle = %f\n", deltaOrientation.GetAngle());
			//printf("remaining difference = %lf\n", fabs(currentOrientation.GetAngle() - desiredOrientation->GetAngle()));
			if(fabs(currentOrientation.GetAngle() - desiredOrientation->GetAngle()) <= fabs(angleChange))
			{
				currentOrientation.SetAngle(desiredOrientation->GetAngle());
				
			}
			else
			{
				//printf("Here 2\n");
				currentOrientation.SetAngle(currentOrientation.GetAngle() + angleChange);
			}
			//printf("New current angle = %f\n", currentOrientation.GetAngle());
			if(fabs(currentOrientation.GetLength() - desiredOrientation->GetLength()) <= fabs(lengthChange))
			{
				currentOrientation.SetLength(desiredOrientation->GetLength());
			}
			else
			{
				currentOrientation.SetLength(currentOrientation.GetLength() + lengthChange);
			}
			

		}
		else
		{
			//times up, put it in the right place
			//printf("times up\n");	
			//currentOrientation.SetAngle(desiredOrientation->GetAngle());
			//currentOrientation.SetLength(desiredOrientation->GetLength());
			//currentOrientation.SetSpeed(0.0);
			//currentOrientation.SetLengthSpeed(0.0);
		}
	}	
	if(outgoingBones)
		outgoingBones->Think(elapsedTime);
}
void Bone::SetDesiredOrientation(BoneOrientationNode * desiredOrientationNode)
{
	SetDesiredOrientation(desiredOrientationNode->GetOrientation());
	if(outgoingBones)
		outgoingBones->SetDesiredOrientation(desiredOrientationNode->GetOutgoingNodeList());	
}
void Bone::SetDesiredOrientation(BoneOrientation * desiredOrientation)
{
	this->desiredOrientation = desiredOrientation;
	float deltaLength = desiredOrientation->GetLength() - currentOrientation.GetLength();
	float deltaAngle =  desiredOrientation->GetAngle() - currentOrientation.GetAngle();

	while(deltaAngle > 360.0)
		deltaAngle -= 360.0;
	while(deltaAngle < -360.0)
		deltaAngle += 360.0;
		
	if(deltaAngle < 0 && deltaAngle+360 < fabs(deltaAngle))
		deltaAngle += 360;
	else if( deltaAngle > 0 && deltaAngle - 360 > deltaAngle)
		deltaAngle -= 360;
	float time = desiredOrientation->GetTime();
	
	accelerating = false;
	deltaOrientation = BoneOrientation(deltaLength, deltaAngle, time);
	
	angleAcceleration = 4.0 * deltaAngle / (time * time);
	lengthAcceleration = 4.0 * deltaLength / (time * time);

	currentOrientation.SetSpeed(deltaAngle/time);
	currentOrientation.SetLengthSpeed(deltaLength/time);
	
	currentOrientation.SetTime(0.0);
}	

bool Bone::CheckOrientation()
{
	return desiredOrientation->GetAngle() == currentOrientation.GetAngle() && desiredOrientation->GetLength() == currentOrientation.GetLength();
}
bool Bone::CheckAllOrientation()
{
	if(outgoingBones)
		return CheckOrientation() && outgoingBones->CheckOrientation();
	return CheckOrientation();
}
void Bone::Draw()
{
	glPushMatrix();
	glRotatef(currentOrientation.GetAngle(), 0,0,1);
	
	//
	
	if(textureManager)
	{
		glColor3f(1,1,1);
		float halfWidth = size.x * 0.5;
		float halfHeight = size.y * 0.5;
		textureManager->EnableTexture(textureIndex);
		glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
		glAlphaFunc( GL_GREATER, 0.01 );
	  	glEnable( GL_ALPHA_TEST );
	  	/*glDisable(GL_TEXTURE_2D);
	  	glColor3f(1,1,1);
		glBegin(GL_LINES);
		glVertex2f(0,0);
		glVertex2f(currentOrientation.GetLength(),0);
		glEnd();*/
		
		
		glEnable(GL_TEXTURE_2D);
		
		glBegin(GL_QUADS);
		//glBegin(GL_LINE_LOOP);
		glTexCoord2f(textureVertices[1].x, textureVertices[1].y);
		glVertex2f(currentOrientation.GetLength() * skinLocation - halfHeight ,halfWidth);
		
		glTexCoord2f(textureVertices[2].x, textureVertices[2].y);
		glVertex2f(currentOrientation.GetLength() * skinLocation + halfHeight,halfWidth);
		
		glTexCoord2f(textureVertices[3].x, textureVertices[3].y);
		glVertex2f(currentOrientation.GetLength() * skinLocation + halfHeight,-halfWidth);
		
		glTexCoord2f(textureVertices[0].x, textureVertices[0].y);
		glVertex2f(currentOrientation.GetLength() * skinLocation - halfHeight,-halfWidth);
		glEnd();
	}
	else
	{
		glColor3f(1,0,0);
		glDisable(GL_TEXTURE_2D);
		glBegin(GL_LINES);
		glVertex2f(0,0);
		glVertex2f(currentOrientation.GetLength(),0);
		glEnd();
	}
	glTranslatef(currentOrientation.GetLength(),0,0);
	if(outgoingBones)
		outgoingBones->Draw();
	glPopMatrix();
}
void Bone::SetModifiers(float angleModifier, float lengthModifier, float timeModifier)
{
	this->angleModifier = angleModifier;
	this->lengthModifier = lengthModifier;
	this->timeModifier = timeModifier;
	
	if(outgoingBones)
		outgoingBones->SetModifiers(angleModifier, lengthModifier, timeModifier);	
}
BoneList * Bone::GetOutgoingBoneList()
{
	return outgoingBones;
}
